using System;
using System.Collections.Generic;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Audio;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.GamerServices;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using Microsoft.Xna.Framework.Net;
using Microsoft.Xna.Framework.Storage;

namespace Autobot
{
    class Observer
    {
        //Graphics things for the observer.
        private SpriteBatch obsBatch;
        private SpriteFont textFont;

        //Graphical objects owned by the observer.
        private VirtualMap map;
        private Robot robot;
        private Goal goal;

        //Hold the window size to place object appropriately
        private Vector2 windowSize;

        // Flag if the observer should be drawn or not
        public bool visible = true;

        private ShowString[] statusStrings;

        enum StatusString : int
        {
            POSITION = 0,
            POSITION_X,
            POSITION_Y,
            DIR,
            DIR_X,
            DIR_Y,
            VELOCITY,
            VELOCITY_VAL,
            MOTOR1,
            MOTOR1_VAL,
            MOTOR2,
            MOTOR2_VAL,
            SENSOR1,
            SENSOR1_VAL,
            SENSOR2,
            SENSOR2_VAL,
            SENSOR3,
            SENSOR3_VAL,
            ANGLE,
            ANGLE_VAL,
            NUM_STRINGS
        }

        public Observer(Vector2 WindowSize)
        {
            //Get the size of the window
            windowSize = WindowSize;

            //CURRENT SETUP for window
            //------------------------------
            //| Pos X Y |   |>             |
            //| Dir X Y |                  |
            //| Mag  X  |                  |
            //| V M1 X  |                  |
            //| V M2 X  |     MAP          |
            //| V S1 X  |                  |
            //| V S2 X  |                  |
            //| V S3 X  |               X  |
            //------------------------------
            statusStrings = new ShowString[(int)StatusString.NUM_STRINGS];
            //Set up the status strings so that they are shown on the left side of the window
            statusStrings[(int)StatusString.POSITION] = new ShowString(new Vector2(0,0),"Pos:",20,Color.Red);
            statusStrings[(int)StatusString.POSITION_X] = new ShowString(new Vector2(50, 0), "0.0", 20, Color.Red);
            statusStrings[(int)StatusString.POSITION_Y] = new ShowString(new Vector2(160, 0), "0.0", 20, Color.Red);

            statusStrings[(int)StatusString.DIR] = new ShowString(new Vector2(0, 30), "Dir:", 20, Color.Red);
            statusStrings[(int)StatusString.DIR_X] = new ShowString(new Vector2(50, 30), "0.0", 20, Color.Red);
            statusStrings[(int)StatusString.DIR_Y] = new ShowString(new Vector2(160, 30), "0.0", 20, Color.Red);
            
            //Magnitude of speed
            statusStrings[(int)StatusString.VELOCITY] = new ShowString(new Vector2(0, 60), "Velocity:", 20, Color.Red);
            statusStrings[(int)StatusString.VELOCITY_VAL] = new ShowString(new Vector2(160, 60), "0.0", 20, Color.Red);
            
            //Motors
            statusStrings[(int)StatusString.MOTOR1] = new ShowString(new Vector2(0, 90), "MD:", 20, Color.Red);
            statusStrings[(int)StatusString.MOTOR1_VAL] = new ShowString(new Vector2(40, 90), "0.0", 20, Color.Red);
            statusStrings[(int)StatusString.MOTOR2] = new ShowString(new Vector2(0, 120), "MS:", 20, Color.Red);
            statusStrings[(int)StatusString.MOTOR2_VAL] = new ShowString(new Vector2(40, 120), "0.0", 20, Color.Red);
            
            //Sensors
            statusStrings[(int)StatusString.SENSOR1] = new ShowString(new Vector2(0, 180), "SL:", 20, Color.Red);
            statusStrings[(int)StatusString.SENSOR1_VAL] = new ShowString(new Vector2(40, 180), "0.0", 20, Color.Red);
            statusStrings[(int)StatusString.SENSOR2] = new ShowString(new Vector2(0, 210), "SM:", 20, Color.Red);
            statusStrings[(int)StatusString.SENSOR2_VAL] = new ShowString(new Vector2(40, 210), "0.0", 20, Color.Red);
            statusStrings[(int)StatusString.SENSOR3] = new ShowString(new Vector2(0, 240), "SR:", 20, Color.Red);
            statusStrings[(int)StatusString.SENSOR3_VAL] = new ShowString(new Vector2(40, 240), "0.0", 20, Color.Red);

            //Angle
            statusStrings[(int)StatusString.ANGLE] = new ShowString(new Vector2(0, 270), "Angle:", 20, Color.Red);
            statusStrings[(int)StatusString.ANGLE_VAL] = new ShowString(new Vector2(80, 270), "0.0", 20, Color.Red);

            //Set up the map to occupy the right 3/4 of the window. Less skewing in the horizontal direction than vertical.
            //The default text font is about 30 pixels high, so the vertical spacing between strings should be about 30.
            Rectangle VirtualMapRect = new Rectangle((int)((1f/4f)*WindowSize.X), 0, (int)((3f/4f)*WindowSize.X), (int)WindowSize.Y);

            //Each block will be representative of 0.1m x 0.1m
            float RealBlockSize = 0.05f;// 1f / 10f; 
            map = new VirtualMap(new Vector2(1.5f,2.0f),new Vector2(RealBlockSize,RealBlockSize),VirtualMapRect,BlockState.UNKNOWN);

            //Set up the robot, make sure it is on the map
            robot = new Robot(new Vector2(VirtualMapRect.X + 10f, VirtualMapRect.Y + 10f), -10f);

            //Set up the goal, make sure it is on the map and not on top of the car
            Vector2 GoalVirtual = new Vector2(VirtualMapRect.X + 80f, VirtualMapRect.Y + 10f);
            Vector2 GoalReal = map.GetRealPosition(GoalVirtual);
            goal = new Goal(GoalVirtual, GoalReal);
        }
        public void Draw()
        {
            // Don't draw anything if the visible flag is false
            if(visible == false)
                return;

            //Make sure to draw map first and then everything else on top
            map.Draw();

            //Test to see if the robot is on the map, and if so draw it
            if (map.CheckBoundaries(robot.position, false) == true)
            {
                robot.Draw();
            }
            //Same with the goal
            if (map.CheckBoundaries(goal.GetVirtualPosition(), false) == true)
            {
                goal.Draw();
            }

            //Draw the status on the left side of the screen
            for (int i = 0; i < (int)StatusString.NUM_STRINGS; i++)
            {
                statusStrings[i].Draw();
            }
        }
        public void Update(GameTime gameTime, Vector2 NewRobotPos,Vector2 NewRobotDir, float NewVelocity, float[] NewSensorData, int[] NewMotorData)
        {
            Vector2 NewVirtualPosition = new Vector2(-1.0f,-1.0f);

            // Get a new virtual position for the robot from the map scaling.
            if (NewRobotPos.X >= 0 && NewRobotPos.Y >=0)
            {
                Vector2 VirtualPos = map.GetVirtualPosition(NewRobotPos);
                NewVirtualPosition = new Vector2(VirtualPos.X,VirtualPos.Y);
            }

            // Update mr. Robot. He will take care of any null pointers and stuff.
            robot.Update(gameTime, NewRobotPos, NewVirtualPosition,NewRobotDir,NewVelocity, NewSensorData, NewMotorData);

            // Update the map with the new sensor readings and the position of the car.
            map.Update(gameTime,NewSensorData,robot.GetRealPosition(),robot.GetDirection());

            // The goal's update check if the robot has reached the destination.
            goal.Update(gameTime, NewRobotPos);

            // Update the status strings variables to the observer window with the new information
            statusStrings[(int)StatusString.POSITION_X].SetString(NewRobotPos.X.ToString());
            statusStrings[(int)StatusString.POSITION_Y].SetString(NewRobotPos.Y.ToString());

            statusStrings[(int)StatusString.DIR_X].SetString(NewRobotDir.X.ToString());
            statusStrings[(int)StatusString.DIR_Y].SetString(NewRobotDir.Y.ToString());

            if (NewSensorData != null)
            {
                statusStrings[(int)StatusString.SENSOR1_VAL].SetString(NewSensorData[(int)SensorID.LEFT].ToString());
                statusStrings[(int)StatusString.SENSOR2_VAL].SetString(NewSensorData[(int)SensorID.MIDDLE].ToString());
                statusStrings[(int)StatusString.SENSOR3_VAL].SetString(NewSensorData[(int)SensorID.RIGHT].ToString());
            }
            if (NewMotorData != null)
            {
                statusStrings[(int)StatusString.MOTOR1_VAL].SetString(NewMotorData[(int)MotorID.DRIVE].ToString());
                statusStrings[(int)StatusString.MOTOR2_VAL].SetString(NewMotorData[(int)MotorID.STEERING].ToString());
            }

            statusStrings[(int)StatusString.VELOCITY_VAL].SetString(NewVelocity.ToString());
            statusStrings[(int)StatusString.ANGLE_VAL].SetString(robot.GetAngle().ToString());
        }
        public bool CheckRobotCollision()
        {
            // We should check a close proximity to the car and see if there is an obstruction
            // Should probably figure out a scaling method to measure how big the car is and 
            // how many layers we should look at.
            int NumBlocks = 0;
            Block[] Blocks = map.GetPaddedBlock(robot.GetVirtualPosition(), 3, ref NumBlocks);
            for (int i = 0; i < NumBlocks; i++)
            {
                if (Blocks[i].CurrentState == BlockState.BLOCKED)
                    return true;
            }
            // We didn't find anything
            return false;

        }
        public Robot GetRobot()
        {
            return robot;
        }
        public VirtualMap GetMap()
        {
            return map;
        }
        public Goal GetDestination()
        {
            return goal;
        }
        public bool CheckGoalReached()
        {
            return goal.CheckGoalReached(robot.GetRealPosition());
        }
        public void LoadContent(ContentManager Content,GraphicsDevice drawDevice)
        {
            //Initialize the car's sprite batch
            obsBatch = new SpriteBatch(drawDevice);

            //Create a sprite font for drawing strings in the observer window
            textFont = Content.Load<SpriteFont>("Courier New");

            //Load content for map,robot and goal
            map.LoadContent(Content,drawDevice);
            robot.LoadContent(Content,drawDevice);
            goal.LoadContent(Content,drawDevice);

            //Load the strings content
            for (int i = 0; i < (int)StatusString.NUM_STRINGS; i++)
            {
                statusStrings[i].LoadContent(Content,drawDevice);
            }
        }
        public void Dispose()
        {
            //Dispose methods for map, robot and goal
            map.Dispose();
            robot.Dispose();
            goal.Dispose();

            //Dispose the strings
            for (int i = 0; i < (int)StatusString.NUM_STRINGS; i++)
            {
                statusStrings[i].Dispose();
            }

            //Dispose of the sprite batches
            if (!obsBatch.IsDisposed)
                obsBatch.Dispose();
        }
    }
}
